#include "navmesh gen.h"
#include <stdlib.h>

#define MAX_TRI_COUNT 256
#define MAX_EDGE_COUNT 128

navGenTri tri[MAX_TRI_COUNT];
navGenEdge edge[MAX_EDGE_COUNT];
int edgeCount;
int triCount;
int vecCount;

int loopIndexTri[3] = {1,2,0};
int loopIndexTri2[3] = {2,0,1};

// Init
void navGenInit(void) {
	navGenReset();
}

// Resets the contents of navgen.
void navGenReset() {
	edgeCount = 0;
	triCount = 0;
	for(int i = 0; i<MAX_TRI_COUNT; i++) {
		for(int j = 0; j<3; j++) {	
			tri[i].v[j] = vec2XY(0,0);
			tri[i].t[j] = NULL;
			tri[i].ts[j] = 0;
		}
		tri[i].path = 0;
	}
	for(int i = 0; i<MAX_EDGE_COUNT; i++) {
		for(int j = 0; j<2; j++) {
			edge[i].v[j] = vec2XY(0,0);
		}
	}
}

// Passes a heap of edges to navgen for usage.
void navGenSetEdges(navGenEdge* e, unsigned int len) {
	for(int i = 0; i<len; i++) {
		int index = edgeCount;
		if(edgeCount < MAX_EDGE_COUNT) {
			edge[index] = e[i];
		}
		edgeCount++;
	}
}

// Internal functions
static int navGenGetNearestPoint(void) {
	if(edgeCount <= 1) return -1;
	navGenEdge e = edge[0];
	float dist = 1024.f;
	int edgeFound = -1;
	int slot = -1;
	for(int i = 1; i<edgeCount; i++) {
		float cmpDist;
		for(int j = 0; j<2; j++) {
			for(int k = 0; k<2; k++) {
				cmpDist = vec2DiffSq(e.v[k],edge[i].v[j]);
				if(cmpDist < dist) {
					dist = cmpDist;
					edgeFound = i;
					slot = j;
				}
			}
		}
	}
	if(edgeFound != -1) {
		return (edgeFound << 1) + slot;
	} else {
		return -1;
	}
}

// Creates a new tri and attached it to prevTri, through edge prevTriEdge.
// Pass -1 into prevTri to create a new tri from edge[0] to connector.
static void navGenCreateTri(int prevTri, int prevTriEdge, vec2 connector) {
	if(triCount >= MAX_TRI_COUNT) return;
	if(prevTri == -1) {
		tri[triCount].v[0] = edge[0].v[0];
		tri[triCount].v[1] = edge[0].v[1];
		tri[triCount].v[2] = connector;
		tri[triCount].t[0] = NULL;
		tri[triCount].t[1] = NULL;
		tri[triCount].t[2] = NULL;
		tri[triCount].ts[0] = 0;
		tri[triCount].ts[1] = 0;
		tri[triCount].ts[2] = 0;
		tri[triCount].path = 0;
	} else {
		if(prevTriEdge == 0) {
			if(tri[prevTri].t[0] != NULL) return;
			tri[triCount].v[0] = tri[prevTri].v[0];
			tri[triCount].v[1] = tri[prevTri].v[1];
			tri[triCount].ts[0] = tri[prevTri].ts[0];
		} else if(prevTriEdge == 1) {
			if(tri[prevTri].t[1] != NULL) return;
			tri[triCount].v[0] = tri[prevTri].v[1];
			tri[triCount].v[1] = tri[prevTri].v[2];
			tri[triCount].ts[0] = tri[prevTri].ts[1];
		} else if(prevTriEdge == 2) {
			if(tri[prevTri].t[2] != NULL) return;
			tri[triCount].v[0] = tri[prevTri].v[2];
			tri[triCount].v[1] = tri[prevTri].v[0];
			tri[triCount].ts[0] = tri[prevTri].ts[2];
		}
		tri[triCount].v[2] = connector;
		tri[triCount].t[0] = &tri[prevTri];
		tri[triCount].t[1] = NULL;
		tri[triCount].t[2] = NULL;
		tri[triCount].ts[1] = 0;
		tri[triCount].ts[2] = 0;
		tri[triCount].path = 0;
	}
	triCount++;
}

// Creates a new tri and attaches it to prevTri1 and prevTri2, using their respective prevTri1Edge and prevTri2Edge.
/* static void navGenCreateTri2(int prevTri1, int prevTri2, int prevTri1Edge, int prevTri2Edge) {
	if(triCount >= MAX_TRI_COUNT) return;
	if(prevTri1 == -1 || prevTri2 == -1) return;
	
	// Find the shared position
	int tri1i[2] = {(prevTri1Edge),loopIndexTri[prevTri1Edge]};
	int tri2i[2] = {(prevTri2Edge),loopIndexTri[prevTri2Edge]};
	
	// i = 0, j = 0
	if(vec2Comp(tri[prevTri1].v[tri1i[0]],tri[prevTri2].v[tri2i[0]])) {
		tri[triCount].v[0] = tri[prevTri1].v[tri1i[0]];
		tri[triCount].v[1] = tri[prevTri1].v[tri1i[1]];
		tri[triCount].v[2] = tri[prevTri2].v[tri2i[1]];
	}
	// i = 0, j = 1
	if(vec2Comp(tri[prevTri1].v[tri1i[0]],tri[prevTri2].v[tri2i[1]])) {
		tri[triCount].v[0] = tri[prevTri1].v[tri1i[0]];
		tri[triCount].v[1] = tri[prevTri1].v[tri1i[1]];
		tri[triCount].v[2] = tri[prevTri2].v[tri2i[0]];
	}
	// i = 1, j = 0
	if(vec2Comp(tri[prevTri1].v[tri1i[1]],tri[prevTri2].v[tri2i[0]])) {
		tri[triCount].v[0] = tri[prevTri1].v[tri1i[1]];
		tri[triCount].v[1] = tri[prevTri1].v[tri1i[0]];
		tri[triCount].v[2] = tri[prevTri2].v[tri2i[1]];
	}
	// i = 1, j = 1
	if(vec2Comp(tri[prevTri1].v[tri1i[1]],tri[prevTri2].v[tri2i[1]])) {
		tri[triCount].v[0] = tri[prevTri1].v[tri1i[1]];
		tri[triCount].v[1] = tri[prevTri1].v[tri1i[0]];
		tri[triCount].v[2] = tri[prevTri2].v[tri2i[1]];
	}
	
	tri[triCount].t[0] = &tri[prevTri1];
	tri[triCount].t[1] = &tri[prevTri2];
	tri[triCount].t[2] = NULL;
	tri[triCount].ts[0] = tri[prevTri1].ts[prevTri1Edge];
	tri[triCount].ts[1] = tri[prevTri2].ts[prevTri1Edge];
	tri[triCount].ts[2] = 0;
	tri[triCount].path = 0;

	triCount++;
}
*/

// Link a given two tri's, over the given edges.
static void navGenLinkTri(int tri1, int tri2, int tri1Edge, int tri2Edge, int linkState) {
	if(tri[tri1].t[tri1Edge] != NULL || tri[tri2].t[tri2Edge] != NULL) return;
	tri[tri1].t[tri1Edge] = &tri[tri2];
	tri[tri2].t[tri2Edge] = &tri[tri1];
	tri[tri1].ts[tri1Edge] = linkState;
	tri[tri2].ts[tri2Edge] = linkState;
}

// Runs navgen.
void navGenRun(void) {
	// Start with wall 0.
	// Get the nearest position to wall 0, create tri 0
	int vFirst = navGenGetNearestPoint();
	if(vFirst == -1) return; // too little tri's
	navGenCreateTri(-1,0,edge[vFirst>>1].v[vFirst&1]);
	
	// For every tri:
	for(int i = 0; i<triCount; i++) {
		// For every edge of tri:
		for(int j = 0; j<3; j++) {
			if(tri[i].t[j] == NULL) {
				// Find the nearest position that is valid
				
				float distSq = 4096.f;
				int nearestE = -1;
				int nearestEI = -1;
				
				// Compare with every single vec2, check if it's closer than our last check, and if yes, check if it's valid by seeing if it crosses with any existing edge
				// Possible optimization: First find the lowest valid value, then go from low->high to for checking
				// this reduces the load of the expensive validity check, and increases the load of the distance check.
				for(int vi = 0; vi<edgeCount; vi++) {
					for(int vj = 0; vj<2; vj++) {
						vec2 v = edge[vi].v[vj];
						int triVi[2] = {j,loopIndexTri[j]};
						vec2 triV[2] = {tri[i].v[triVi[0]],tri[i].v[triVi[1]]};
						
						// distance comparison
						float diff = vec2DiffSq(v,triV[0]);
						float diff2 = vec2DiffSq(v,triV[1]);
						if(diff < distSq || diff2 < distSq) {
							if(diff2 < diff) diff = diff2; // use shortest of the two distances
							if(diff <= 0.f) goto fail;
							if(vec2Comp(v,tri[i].v[loopIndexTri2[j]])) goto fail;
							
							{
								// check if the new triangle will be completely flat (2 slopes to new point are equal)
								vec2 vDiff = vec2Sub(v,triV[0]);
								float diffSlope = fabs(vDiff.y/vDiff.x);
								vec2 vDiff2 = vec2Sub(v,triV[1]);
								float diff2Slope = fabs(vDiff2.y/vDiff2.x);
								if(vDiff.x == 0 && vDiff2.x == 0) {
									goto fail;
								}
								if(diffSlope == diff2Slope && diffSlope == diff2Slope) {
									goto fail;
								}
							}
							
							{
								//check if the point is on the correct side of the edge (direction between edge and oldtri[3] and edge and new point are opposite (<90deg))
								vec2 triV3 = tri[i].v[loopIndexTri2[j]];
								vec2 avg = vec2Scale(vec2Add(triV[0],triV[1]),0.5f);
								vec2 cmpLine = vec2Sub(triV[0],avg);
								cmpLine = vec2XY(-cmpLine.y,cmpLine.x);
								
								triV3 = vec2Sub(triV3,avg);
								vec2 comp = vec2Sub(v,avg);
								
								float dot1 = vec2Dot(triV3,cmpLine);
								float dot2 = vec2Dot(comp,cmpLine);
								float dot = dot1*dot2;
								if(dot >= 0.f) {
									//return;
									goto fail;
								}
							}
								
							
							// new tri is v,triV[0],triV[1]. The edges from v to triV[0] and v to triV[1] need to be checked for validity. (No intersection)
							for(int k = 0; k<2; k++) {
								for(int triCmp = 0; triCmp<triCount; triCmp++) {
									for(int triCmpE = 0; triCmpE<3; triCmpE++) {
										// possible optimization: abuse that tri's are in order in memory
										int triCmpE2 = loopIndexTri[triCmpE];
										if(vec2Cross(v,triV[k],tri[triCmp].v[triCmpE],tri[triCmp].v[triCmpE2])) {
											goto fail; // Goto to break multiple loops and skip the need for a "success" variable
										}
									}
								}
								for(int edgeCmp = 0; edgeCmp<edgeCount; edgeCmp++) {
									if((!vec2Comp(edge[edgeCmp].v[0],v)) && (!vec2Comp(edge[edgeCmp].v[1],v)) && (!vec2Comp(edge[edgeCmp].v[0],triV[k])) && (!vec2Comp(edge[edgeCmp].v[1],triV[k]))) {
										if(vec2Cross(v,triV[k],edge[edgeCmp].v[0],edge[edgeCmp].v[1])) {
											goto fail;
										}
									}
								}
							}
							// success! This one's valid!
							distSq = diff;
							nearestE = vi;
							nearestEI = vj;
							
							fail:{}
						}
					}
				}
				
				if(nearestE >= 0) {
					// Create new tri
					navGenCreateTri(i,j,edge[nearestE].v[nearestEI]);
					int newTriI = triCount-1;
					// Try to merge edges with existing tri
					for(int newTriE = 1; newTriE < 3; newTriE++) {
						for(int k = triCount-2; k>=0; k--) {
							for(int triE = 0; triE < 3; triE++) {
								if(tri[k].t[triE] == NULL) {
									if(vec2Comp(tri[k].v[triE],tri[newTriI].v[newTriE]) || vec2Comp(tri[k].v[loopIndexTri[triE]],tri[newTriI].v[newTriE])) {
										if(vec2Comp(tri[k].v[triE],tri[newTriI].v[loopIndexTri[newTriE]]) || vec2Comp(tri[k].v[loopIndexTri[triE]],tri[newTriI].v[loopIndexTri[newTriE]])) {
											// merge
											tri[newTriI].t[newTriE] = &tri[k];
											tri[k].t[triE] = &tri[newTriI];
											goto break2;
										}
									}
								}
							}
						}
						break2:{}
					}
					// for every existing tri
						// for every edge of the new tri
							// for ever edge of this tri
								// if tri matches not linked
									// if vertices match
										// link tri edges
					if(triCount >= MAX_TRI_COUNT) goto exitLoop; // Goto to break multiple loops
				}
			}
		}
	}
	exitLoop:{}
	
	// For every tri:
		// Compare corner vertices with wall vertices
		// if match is found
			// mark edge unpathable
		
		// get every edge len
		// if len < x(robot size)
			// mark edge unpathable
		//
	
}


// Gets the amount of navgen Triangles that were generated.
unsigned int navGenGetTriCount(void) {
	return triCount;
}
// Fetches a navgen triangle. The id must be smaller than navGenGetTriCount();
navGenTri* navGenGetTri(unsigned int id) {
	return &tri[id];
}
// Fetches the navgen triangle in which the position can be found, and writes it to the given pointer. Returns 0 on success and 1 on failure.
int navGenGetTriPos(vec2 pos, navGenTri* t);

/* int main(int argc, char** argv) {
	navGenEdge walls[6] = {
		{{
			{0,0},
			{0,1},
		}},
		{{
			{1,2},
			{2,3},
		}},
		{{
			{-1,-1},
			{-1,2},
		}},
		{{
			{-5,4},
			{10,3.5},
		}},
		{{
			{-5,-3},
			{6,-4},
		}},
		{{
			{7,-2},
			{7,3},
		}}
	};
	
	navGenInit();
	navGenReset();
	navGenSetEdges(walls,6);
	navGenRun();
	// fetch data from navgen
	// print it out or something
	
	printf("Tri's found: %d\n",triCount);
	for(int i = 0; i<triCount; i++) {
		printf("%d:\n",i);
		for(int j = 0; j<3; j++) {
			vec2Print(tri[i].v[j]);
		}
	}
	
	return 0;
}*/

